#ifndef __COMMAND_H__
#include "Command.h"
#endif //__COMMAND_H__

#ifndef __C3DFILEINFO_H__
#include "C3dFileInfo.h"
#endif	//__C3DFILEINFO_H__

#ifndef __ARTICULATEDBODY_H__
#include "ArticulatedBody.h"
#endif	//__ARTICULATEDBODY_H__

#ifndef RealTimeIKui_h
#include "RealTimeIKui.h"
#endif //RealTimeIKui_h

#ifndef __PHYLTERFLGLWINDOW_H__
#include "PhylterGLWindow.h"
#endif	//__PHYLTERFLGLWINDOW_H__

#ifndef	__PHOWARDDATA_H__
#include "PhowardData.h"
#endif

#ifndef __TRANSFORM_H__
#include "Transform.h"
#endif	//__TRANSFORM_H__

#include "vl/Mat.h"

#include <deque>


int readSkelFile( FILE* file, ArticulatedBody* skel );

extern RealTimeIKUI *UI;

void LoadModel(void *v)
{
  char *params = (char*)v;
  if(!params){
    params = (char*)fl_file_chooser("Open File?", "{*.skel}", "src/skels" );
  }

  if(!params)
    return;

  FILE *file = fopen(params, "r");
    
  if(file == NULL){
    cout << "Skel file does not exist" << endl;
    return;
  }

  ArticulatedBody *mod = new ArticulatedBody();
  UI->mData->mModels.push_back(mod);
  UI->mData->mSelectedModel = mod;

  readSkelFile(file, mod);
  UI->CreateDofSliderWindow();

  mod->InitModel();
  UI->mGLWindow->mShowModel = true;
  UI->mShowModel_but->value(1);
  UI->mGLWindow->refresh();
  
  cout << "number of dofs in model: " << UI->mData->mModels[0]->GetDofCount() << endl;
}

double magnitude(Vec3d x) {
	return sqrt( x[0]*x[0] + x[1]*x[1] + x[2]*x[2] );
}

//Compute F.  Since q is global information no parameters
double f() {
	
	int frame = UI->mFrameCounter_cou->value();

	double sum = 0.0;

	//Get model and list of markers
	Model *model = UI->mData->mSelectedModel;
	vector<Marker*> handleList = model->mHandleList;

	for(int i=0; i < model->GetHandleCount(); i++)
	{
		Vec3d pi = model->mOpenedC3dFile->GetMarkerPos(frame,i);
		if(magnitude(pi) > 0) {
			Vec3d Ci = handleList[i]->mGlobalPos - pi;
			sum += magnitude(Ci);
		}
	}

	return sum;
}

vector<Vecd*>cache;
vector<bool>valid;

void Solution(void *v)
{
	
	Model *model = UI->mData->mSelectedModel;

	//Create cache
	if(cache.size() == 0) {
		int frames = model->mOpenedC3dFile->GetFrameCount();
		cache.resize(frames);
		valid.resize(frames);
		for(int i=0;i<frames;i++)
			valid[i] = false;
	}

	int frame = UI->mFrameCounter_cou->value();

	if(valid[frame])
		model->mDofList.SetDofs(*cache[frame]);

    cout << "TODO: Solve inverse kinematics problem" << endl;
    bool test = UI->mData->mSelectedModel->mLimbs[0]->mTransforms[0]->IsDof();

	//Get model and list of markers
	vector<Marker*> handleList = model->mHandleList;

	for(int dof=0; dof<model->GetDofCount(); dof++)
		if(model->mDofList.GetDof(dof) == 0)
			model->mDofList.SetDof(dof, 0.1);

	//while the marker is off target more than epsilon run minimization loop
	double epsilon = 0.01;

	int steps=0;
	while( f() > epsilon )
	{
		double alpha = 1;

		Matd j(model->GetHandleCount()*3, model->GetDofCount(), ZeroOrOne::vl_zero);
		//TODO Compute Jacobian
		
		for(int markIndex = 0; markIndex < model->GetHandleCount(); markIndex++) {
			Marker *mark = model->mHandleList[markIndex];

			if(magnitude(model->mOpenedC3dFile->GetMarkerPos(frame,markIndex)) < 0.01) {
				cout<< "Avoided Missing marker "<< model->mOpenedC3dFile->GetMarkerPos(frame,markIndex) << endl;
				continue;
			}

			TransformNode *node = model->mLimbs[mark->mNodeIndex];
			Vec4d children(mark->mOffset,1);
		
			while(node != NULL) {

				//Get parent Transform
				Mat4d parent = node->mParentTransform;
				
				for(int tIndex=0; tIndex < node->GetSize(); tIndex++) {
					Transform *current = node->mTransforms[tIndex];

					if(!current->IsDof()) continue;

					for(int dof=0; dof < current->GetDofCount(); dof++) {
						Mat4d result = parent;

					
						//get transforms before this one
						for(int k=0; k<tIndex; k++)
							result = result * node->mTransforms[k]->GetTransform();

						//Get this derivative
						result = result * current->GetDeriv(dof);

						//get transforms after this one
						for(int k=tIndex+1; k < node->mTransforms.size(); k++)
							result = result * node->mTransforms[k]->GetTransform();
					

						//get children
						Vec4d Ji = result * children;

						int col = current->GetDof(dof)->mId;

						//cout << col << Ji<<endl;
						j[markIndex*3+0][col] = Ji[0];
						j[markIndex*3+1][col] = Ji[1];
						j[markIndex*3+2][col] = Ji[2];
					}
				}

				children = node->mLocalTransform * children;

				node = node->mParentNode;
			}
		}
		//Compute sudo inverse of Jacobian
		//Matd jt = inv( trans(j)*j ) * trans(j);
		Matd jt = trans(j);
		
		//compute C
		Vecd c(3 * model->GetHandleCount());
		for(int i=0; i<model->GetHandleCount(); i++) {
			Vec3d pi = model->mOpenedC3dFile->GetMarkerPos(frame,i);
			Vec3d ci = handleList[i]->mGlobalPos - pi;

			c[0+i*3] = ci[0];
			c[1+i*3] = ci[1];
			c[2+i*3] = ci[2];
		}

		ofstream out("testing",ofstream::app);
		out <<"J\n" << j<<endl;
		out <<"C\n" << c<<endl;
		out <<"Jt\n" << jt<<endl;
		cout << "Saved Jacobian\n";

		Vecd old(model->GetDofCount());
		model->mDofList.GetDofs(&old);

		Vecd deriv = 2*jt*c;

		double error = f();
		while(f() >= error) {
			Vecd newq = old - alpha * deriv;

			//save new q using SetDof
			model->SetDofs( newq );
			alpha -= 0.09;

			if(alpha < -1) {
				cout << "No good alpha " << f() << endl;
				cache[frame] = new Vecd(newq);
				valid[frame] = true;
				return;
			}
		}
		cout << "Alpha: "<<alpha+0.09<< endl;

		cout << "Iterations: " << ++steps << "Error: " << f() << endl;
		
		if(v!=NULL && steps > 4) {
			Vecd *store = new Vecd(model->GetDofCount());
			model->mDofList.GetDofs(store);
			cache[frame] = store;
			valid[frame] = true;
			return;
		}
	} 
	Vecd *store = new Vecd(model->GetDofCount());
	model->mDofList.GetDofs(store);
	cache[frame] = store;
	valid[frame] = true;
}

void Exit(void *v)
{
  exit(0);
}

void LoadC3d(void *v)
{
  if(!UI->mData->mSelectedModel){
    cout << "Load skeleton first";
    return;
  }
  char *params = (char*)v;
  if(!params){
    params = fl_file_chooser("Open File?", "{*.c3d}", "src/mocap/" );
  }

  if(!params)
    return;
  
  char *c3dFilename = new char[80];
  
  // load single c3d file
 
  C3dFileInfo *openFile = new C3dFileInfo(params);
  openFile->LoadFile();
  UI->mData->mSelectedModel->mOpenedC3dFile = openFile;
  cout << "number of frames in c3d: " << openFile->GetFrameCount() << endl;

  UI->InitControlPanel();
  UI->mGLWindow->mShowConstraints = true;
  UI->mShowConstr_but->value(1);
}
